# encoding: utf-8


import matplotlib.pyplot as plt
import numpy as np
import math


plt.rcParams['font.sans-serif']=['SimHei'] # 用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False # 用来正常显示负号


# 生产标准梯形曲线 设定起始速度 结束速度 运动距离
def generateTrapPos(startVel,endVel,acc,dec,maxVel,Pos):
    ret = np.zeros(1000, dtype=np.float64)

    return ret

# 加速度单位mm/s^2 速度单位mm/s 距离单位 mm
def generateStandardTrapPos(acc,dec,maxVel,pos):

    t1 = 0.0
    t2 = 0.0
    t3 = 0.0
    vel = maxVel
    # 假设能达到最大速度，计算能达到最大速度的临界距离
    t1 = vel / acc
    t3 = vel / dec
    t2 = 0.0
    tempPos = (t1 + t3) * vel / 2

    # 不能达到最大速度
    if tempPos > pos :
        vel = math.sqrt(pos / (0.5 * acc + 0.5 * dec))
        t1 = vel / acc
        t2 = 0.0
        t3 = vel / dec
    else :
        t2 = (pos - tempPos) / vel

    pos1 = 0.5 * vel * t1
    pos2 = vel * t2
    pos3 = pos - pos1 -pos2

    # 根据t1 t2 t3 v 按时间等分为len份，求出位置点 
    t = (t1 + t2 + t3)  
    subTime = t / 1000


    #累计计算时间
    currentTime = 0.0 
    i = 0
    currentVel = 0.0

    retPos = np.zeros(1001, dtype=np.float64)
    # 加速段计算
    while currentTime <= t1 :
        currentVel = acc * currentTime
        retPos[i] = 0.5 * currentVel * currentTime
        currentTime = currentTime + subTime
        i = i + 1

    # 匀速段计算
    while currentTime <= t1 + t2 :
        if currentVel < vel :
            tempTime = (vel - currentVel) / acc
            currentVel = vel
            retPos[i] = pos1 + (subTime - tempTime) * vel
            currentTime = currentTime + subTime
            i = i + 1
        else :
            currentVel = vel
            retPos[i] = pos1 + (currentTime - t1) * vel
            currentTime = currentTime + subTime
            i = i + 1   

    # 减速段计算
    while i < 1001 :        
        currentVel = vel - (currentTime - t1 -t2) * dec
        retPos[i] = pos1 + pos2 + 0.5 * (currentTime - t1 - t2) * (vel + currentVel)
        currentTime = currentTime + subTime
        i = i + 1 

    # re = np.zeros((1001,2), dtype=np.float)
    # re[:,0] = retVel
    # re[:,1] = retPos
    return retPos

   
# 求导数
def meanFilter(pos,len) :
    size = pos.shape[0]
    reSize = size + len
    re = np.zeros(reSize,dtype=np.float64)
    startPos = 0.0
    totalPos = 0.0
    for i in range(len) :
        totalPos = totalPos + pos[i]
        re[i] = totalPos / len

    for j in range(size-len) :
        d = len + j
        totalPos = totalPos + pos[d] - pos[j]
        re[d] = totalPos / len

    for k in range(len) :
        d = size - len + k
        a = size + k
        totalPos = totalPos + pos[size - 1] - pos[d]
        re[a] = totalPos / len
    return re


def derivative(data):
    size = data.shape[0]
    re = np.zeros(size,dtype=np.float64)
    for i in range(size-1):
        re[i] = data[i+1]-data[i]
    re[size-1] = 0    
    return re

file_name = "梯形曲线"
plt.figure(file_name)


data = generateStandardTrapPos(20.0,20.0,100.0,1000.0)



x = np.zeros(1001,dtype=np.int32)
for i in range(1001):
    x[i] = i
y = data
ax = plt.subplot(231)  # 创建一个二维的绘图工程
ax.set_title('滤波前-位置曲线')  # 设置本图名称
ax.plot(x, y)
# ax.set_xlabel('X')  # 设置x坐标轴
# ax.set_ylabel('Y')  # 设置y坐标轴

y = derivative(data)
ax = plt.subplot(232)  # 创建一个二维的绘图工程
ax.set_title('滤波前-速度曲线')  # 设置本图名称
ax.plot(x, y)
# ax.set_xlabel('X')  # 设置x坐标轴
# ax.set_ylabel('Y')  # 设置y坐标轴

filter_len = 200
filter_size = 1001 + filter_len
fx = np.zeros(filter_size,dtype=np.int)
for i in range(filter_size):
    fx[i] = i
y = meanFilter(data,filter_len)
ax = plt.subplot(234)  # 创建一个二维的绘图工程
ax.set_title('滤波后-位置曲线')  # 设置本图名称
ax.plot(fx, y)
# ax.set_xlabel('X')  # 设置x坐标轴
# ax.set_ylabel('Y')  # 设置y坐标轴

y = derivative(y)
ax = plt.subplot(235)  # 创建一个二维的绘图工程
ax.set_title('滤波后-速度曲线')  # 设置本图名称
ax.plot(fx, y)
# ax.set_xlabel('X')  # 设置x坐标轴
# ax.set_ylabel('Y')  # 设置y坐标轴

y = derivative(y)
ax = plt.subplot(236)  # 创建一个二维的绘图工程
ax.set_title('滤波后-加速度曲线')  # 设置本图名称
ax.plot(fx, y)
# ax.set_xlabel('X')  # 设置x坐标轴
# ax.set_ylabel('Y')  # 设置y坐标轴

plt.show()

